package com;


import com.common.enums.PX4CustomMainMode;
import com.utils.UDPInputStream;
import com.utils.UDPOutputStream;
import com.common.enums.PX4CustomSubModeAuto;
import io.dronefleet.mavlink.Mavlink2Message;
import io.dronefleet.mavlink.MavlinkConnection;
import io.dronefleet.mavlink.MavlinkMessage;
import io.dronefleet.mavlink.common.*;
import io.dronefleet.mavlink.minimal.*;
import io.dronefleet.mavlink.util.EnumValue;
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.net.DatagramSocket;
import java.util.ArrayList;
import java.util.Scanner;

import static com.MAVLink.common.msg_home_position.MAVLINK_MSG_ID_HOME_POSITION;
import static com.MAVLink.common.msg_local_position_ned.MAVLINK_MSG_ID_LOCAL_POSITION_NED;

public class SocketTest {

    public static void main(String[] args) throws IOException, InterruptedException {
        try (DatagramSocket socket = new DatagramSocket(8080);
             InputStream in = new UDPInputStream(socket);
             OutputStream out = new UDPOutputStream(socket, "192.168.1.123", 8080);) {
            // 在建立连接后，我们创建一个MavlinkConnection实例。
            MavlinkConnection connection = MavlinkConnection.create(in, out);

            // 现在我们准备好读取和发送消息。
//            yaw(connection,1,1,0);
//            RequestLocalPositionNed(connection, 1, 1);
            sendTakeoffCommand(connection, 1, 1, 10);
            while(true){
                Scanner scanner = new Scanner(System.in);
                rc(connection, 1, 1,scanner.nextInt(),scanner.nextInt(),scanner.nextInt(),scanner.nextInt());
            }
////            MavlinkMessage message;
//            HomePosition homePosition = null;
//            while ((message = connection.next()) != null) {
//                // 接收到的消息可以是Mavlink1消息，也可以是Mavlink2消息。
//                // 要检查消息是否是Mavlink2消息，我们可以这样做：
//                if (message instanceof Mavlink2Message) {
//                    // 这是一个Mavlink2消息。
//                    Mavlink2Message message2 = (Mavlink2Message) message;
//                    if (message2.isSigned()) {
//                        // 这是一个签名消息。让我们验证它的签名。
//                        if (message2.validateSignature("mySecretKey".getBytes())) {
//                            // 签名有效。
//                        } else {
//                            // 签名验证失败。这条消息是可疑的，
//                            // 不应该被处理。也许我们应该记录这个事件。
//                        }
//                    } else {
//
//                    }
//                } else {
//                    // 这是一个Mavlink1消息。
//                }
//                // 当接收到消息时，其有效负载类型并不是静态可用的。
//                // 我们可以通过其有效负载来解析是哪种消息，像这样：
//                if (message.getPayload() instanceof Heartbeat) {
//                    // 这是一个心跳消息
//                    MavlinkMessage<Heartbeat> heartbeatMessage = (MavlinkMessage<Heartbeat>) message;
//                    System.out.println(message.getPayload());
//                } else if (message.getPayload() instanceof LocalPositionNed) {
//                    LocalPositionNed localPositionNed = (LocalPositionNed) message.getPayload();
//                    System.out.printf("local position, [x,y,z] = [%f,%f,%f], sys_id:%d\n", localPositionNed.x(), localPositionNed.y(), localPositionNed.z(), message.getOriginSystemId());
//                } else if (message.getPayload() instanceof  HomePosition){
//                     homePosition = (HomePosition) message.getPayload();
//                    System.out.printf("home position , [x,y,z] = [%f,%f,%f]\n" , homePosition.x(),homePosition.y(),homePosition.z());
//                    break;
//                } else {
//                    System.out.println("No");
//                }
//                // 我们最好将有效负载发布到像RxJava、JMS或任何其他喜欢的发布/订阅机制中。
//            }
//            Thread.sleep(5000);
//            flyRectangle(connection, 1, 1, 10, 10, homePosition.x(), homePosition.y(), true);
//            flyCircle(connection,1,1,0.5f,homePosition.x(), homePosition.y(), false);
//            flyPointToPoint(connection,1,1,homePosition.x(),homePosition.y(), homePosition.x()+50, homePosition.y());
//            flyRectangleByP2P(connection,1,1,30,30, homePosition.x(), homePosition.y(), false);
        } catch (Exception eof) {
            throw eof;
            // 流已经结束。
        }
    }


    /**
     * 自动起飞模式
     *
     * @param connection
     * @param systemId
     * @param componentId
     * @param altitude
     * @throws IOException
     */
    public static void sendTakeoffCommand(MavlinkConnection connection, int systemId, int componentId, float altitude) throws IOException {
        SetMode setMode = SetMode.builder()
                .targetSystem(1)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_AUTO.getValue() << 16) | (PX4CustomSubModeAuto.PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF.getValue() << 24))
                .build();
        connection.send2(1, 1, setMode);
    }

    public static void setAutoReturnMode(MavlinkConnection connection, int systemId, int componentId, float altitude) throws IOException {
        SetMode setMode = SetMode.builder()
                .targetSystem(1)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_AUTO.getValue() << 16) | (PX4CustomSubModeAuto.PX4_CUSTOM_SUB_MODE_AUTO_RTL.getValue() << 24))
                .build();
        connection.send2(systemId, componentId, setMode);
    }

    /**
     * 降落指令
     *
     * @param connection
     * @param systemId
     * @param componentId
     * @param altitude
     * @throws IOException
     * @throws InterruptedException
     */
    public static void sendLandCommand(MavlinkConnection connection, int systemId, int componentId, float altitude) throws IOException, InterruptedException {
        Thread.sleep(5000);
        CommandLong landCommand = CommandLong.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .command(MavCmd.MAV_CMD_NAV_LAND)
                .build();
        connection.send2(systemId, componentId, landCommand);
    }


    /**
     * 发送心跳包
     *
     * @param connection
     * @param systemId
     * @param componentId
     * @throws IOException
     */
    public static void sendHeartbeat(MavlinkConnection connection, int systemId, int componentId) throws IOException {
        Heartbeat heartbeat = Heartbeat.builder()
                .type(MavType.MAV_TYPE_QUADROTOR)
                .autopilot(MavAutopilot.MAV_AUTOPILOT_PX4)
                .systemStatus(MavState.MAV_STATE_STANDBY)
                .mavlinkVersion(3)
                .build();
        // Write an unsigned heartbeat
        connection.send2(systemId, componentId, heartbeat);
    }

    /**
     * 设置板外模式
     *
     * @param connection
     * @param systemId
     * @param componentId
     * @throws IOException
     */
    public static void setOffBoardMode(MavlinkConnection connection, int systemId, int componentId) throws IOException {

        SetMode setMode = SetMode.builder()
                .targetSystem(systemId)
                .baseMode(MavModeFlag.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
                .customMode((long) PX4CustomMainMode.PX4_CUSTOM_MAIN_MODE_OFFBOARD.getValue() << 16)
                .build();
        // 发送SET_MODE消息
        connection.send2(systemId, componentId, setMode);
    }

    /**
     * 请求飞机当前位置信息
     *
     * @param connection
     * @param systemId
     * @param componentId
     * @throws IOException
     */
    public static void RequestLocalPositionNed(MavlinkConnection connection, int systemId, int componentId) throws IOException {
        RequestDataStream requestDataStream = RequestDataStream.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .reqStreamId(MAVLINK_MSG_ID_LOCAL_POSITION_NED)
                .reqMessageRate(10)
                .startStop(0)
                .build();
        connection.send2(systemId, componentId, requestDataStream);
    }

    public static void flyRectangle(MavlinkConnection connection, int systemId, int componentId, float length, float width, float current_x, float current_y, boolean clockwise) throws InterruptedException, IOException {
        // 飞行高度
        float flight_altitude = -5.0f;

        // 轨迹信息发送频率
        float rate = 20.0f;

        // 完成一圈矩形的时间
        float cycle_s = 40.0f;

        int steps = (int) (cycle_s * rate);

        float dt = 1.0f / rate;

        int timeNowMs = 0;
        int i = 0;

        // 确定矩形的四个顶点
        float[][] points = {
                {0, 0},
                {length, 0},
                {length, width},
                {0, width}
        };

        if (!clockwise) {
            // 如果逆时针飞行，反转顶点顺序
            points = new float[][]{
                    {0, 0},
                    {0, width},
                    {length, width},
                    {length, 0}
            };
        }

        while (i < steps) { // Corrected loop condition to execute only one loop
            // 计算当前线段的起点和终点
            int segment = (i / (steps / 4)) % 4;
            float[] start = points[segment];
            float[] end = points[(segment + 1) % 4];

            // 计算当前点在起点和终点之间的插值
            float t = (float) (i % (steps / 4)) / (steps / 4);
            float x = start[0] + t * (end[0] - start[0]);
            float y = start[1] + t * (end[1] - start[1]);

            // 计算当前飞行方向的偏航角（弧度）
            float yaw = (float) Math.atan2(end[1] - start[1], end[0] - start[0]);

            i++;

            // 创建目标位置消息
            SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                    .timeBootMs((int) (timeNowMs))
                    .targetSystem(1)
                    .targetComponent(1)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE
                    )
                    .x(current_x + x)
                    .y(current_y + y)
                    .z(flight_altitude)
                    .vx(0)
                    .vy(0)
                    .afx(0)
                    .afy(0)
                    .yaw(yaw)
                    .build();
            connection.send2(systemId, componentId, targetLocal);
//            System.out.println("x:" + x + "   y:" + y + "   yaw:" + yaw);
            timeNowMs += 1000 / rate;

            // 等待一段时间
            Thread.sleep((long) (1000 / rate));
        }
        sendLandCommand(connection, 1, 1, 10f);
    }

    public static void fly8(MavlinkConnection connection, int systemId, int componentId) throws IOException, InterruptedException {
        final float PI = 3.14159265359f;
        final float RATE = 20.0f; // 根据你的实际情况设置
        final float dt = 1.0f / RATE;
        final float CYCLE_S = 10.0f; // 根据你的实际情况设置
        final float dadt = (2.0f * PI) / CYCLE_S;
        final float RADIUS = 5.0f; // 根据你的实际情况设置
        final int STEPS = 100; // 根据你的实际情况设置
        float FLIGHT_ALTITUDE = 10.0f; // 根据你的实际情况设置
        int i = 0;
        long timeNowMs = 0;
        while (true) {
            float a = (-PI / 2.0f) + i * (2.0f * PI / STEPS);
            float c = (float) Math.cos(a);
            float c2a = (float) Math.cos(2.0 * a);
            float c4a = (float) Math.cos(4.0 * a);
            float c2am3 = c2a - 3.0f;
            float c2am3_cubed = c2am3 * c2am3 * c2am3;
            float s = (float) Math.sin(a);
            float cc = c * c;
            float ss = s * s;
            float sspo = (s * s) + 1.0f;
            float ssmo = (s * s) - 1.0f;
            float sspos = sspo * sspo;

            i = (i + 1) % STEPS;

            SetPositionTargetLocalNed targetLocal = new SetPositionTargetLocalNed.Builder()
                    .targetSystem(1)
                    .targetComponent(1)
                    .timeBootMs((int) timeNowMs)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(  PositionTargetTypemask.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE)
                    .x(-(RADIUS * c * s) / sspo)
                    .y((RADIUS * c) / sspo)
                    .z(FLIGHT_ALTITUDE)
                    .vx(dadt * RADIUS * (ss * ss + ss + (ssmo * cc)) / sspos)
                    .vy(-dadt * RADIUS * s * (ss + 2.0f * cc + 1.0f) / sspos)
                    .afx(-dadt * dadt * 8.0f * RADIUS * s * c * ((3.0f * c2a) + 7.0f) / c2am3_cubed)
                    .afy(dadt * dadt * RADIUS * c * ((44.0f * c2a) + c4a - 21.0f) / c2am3_cubed)
                    .yaw((float) Math.atan2(-dadt * RADIUS * s * (ss + 2.0f * cc + 1.0f) / sspos, dadt * RADIUS * (ss * ss + ss + (ssmo * cc)) / sspos))
                    .build();

            connection.send2(1,1,targetLocal);

            timeNowMs += 1000 / RATE;

            // Sleep for the duration of the time step
            Thread.sleep((long) (1000 / RATE));

            // Exit the loop after 1 minute of flight
            if (timeNowMs >= 60 * 1000) {
                break;
            }
        }

        // Wait for 3 seconds to enter Hold mode and decelerate
        Thread.sleep(3000);

    }

    public static void flyCircle(MavlinkConnection connection, int systemId, int componentId, float radius, float current_x, float current_y, boolean clockwise) throws InterruptedException, IOException {
        // 飞行高度
        float flight_altitude = -10f;

        // 轨迹信息发送频率
        float rate = 20.0f;

        // 完成一圈圆形的时间
        float cycle_s = 80.0f;

        int steps = (int) (cycle_s * rate);

        float dt = 1.0f / rate;

        int timeNowMs = 0;
        int i = 0;

        int directionFactor = clockwise ? 1 : -1; // 顺时针为1，逆时针为-1

        while (i < steps) {
            // 计算当前位置的参数
            float t = i * dt;
            float angle = directionFactor * 2.0f * (float) Math.PI * t / cycle_s;
            float x = radius * (float) Math.cos(angle);
            float y = radius * (float) Math.sin(angle);
            float yaw = clockwise ? angle + (float) Math.PI / 2.0f: +angle - (float) Math.PI / 2.0f;

            // 创建目标位置消息
            SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                    .timeBootMs(timeNowMs)
                    .targetSystem(1)
                    .targetComponent(1)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VX_IGNORE ,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VY_IGNORE ,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE ,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AX_IGNORE ,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AY_IGNORE ,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE)
                    .x(current_x + x - radius)
                    .y(current_y + y)
                    .z(flight_altitude)
                    .yaw(yaw)
                    .build();

            // 发送目标位置消息
            connection.send2(systemId, componentId, targetLocal);

//            System.out.println("x:" + (current_x + x - radius) + "   y:" + (current_y + y) + "   yaw:" + yaw);
            timeNowMs += 1000 / rate;

            // 等待一段时间
            Thread.sleep((long) (1000 / rate));
            i++;
        }
        sendLandCommand(connection, 1, 1, 10f);
    }

    public static void getHomePosition(MavlinkConnection connection, int systemId, int componentId) throws IOException {
        CommandLong getHomePositionCommand = CommandLong.builder()
                .targetSystem(systemId)
                .targetComponent(componentId)
                .command(MavCmd.MAV_CMD_REQUEST_MESSAGE)
                .param1(MAVLINK_MSG_ID_HOME_POSITION)
                .build();
        connection.send2(systemId,componentId,getHomePositionCommand);
    }

    public static void flyPointToPoint(MavlinkConnection connection, int systemId, int componentId,
                                       float startX, float startY, float endX, float endY) throws InterruptedException, IOException {
        // 飞行高度
        float flight_altitude = -10.0f;

        // 轨迹信息发送频率
        float rate = 20.0f;

        // 完成飞行的时间（秒）
        float duration_s = 10.0f;

        int steps = (int) (duration_s * rate);

        float dt = 1.0f / rate;

        int timeNowMs = 0;


        for (int i = 0; i < steps; i++) {
            // 计算当前点在起点和终点之间的插值
            float t = (float) i / steps;
            float x = startX + t * (endX - startX);
            float y = startY + t * (endY - startY);

            // 计算当前飞行方向的偏航角（弧度），头朝前飞行
            float yaw = (float) Math.atan2(endY - startY, endX - startX);

            // 创建目标位置消息
            SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                    .timeBootMs((int) (timeNowMs))
                    .targetSystem(1)
                    .targetComponent(1)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE)
                    .x(x)
                    .y(y)
                    .z(flight_altitude)
                    .yaw(yaw)
                    .build();
            connection.send2(systemId, componentId, targetLocal);

            timeNowMs += 1000 / rate;

            // 等待一段时间
            Thread.sleep((long) (1000 / rate));
        }
    }
    public static void flyRectangleByP2P(MavlinkConnection connection, int systemId, int componentId, float length, float width, float current_x, float current_y, boolean clockwise) throws InterruptedException, IOException {
        // 飞行高度
        float flight_altitude = -10.0f;

        int timeNowMs = 0;

        float yaw = (float) Math.toRadians(90);

        if (!clockwise) {
            yaw = -yaw;
        }

        // 确定矩形的四个顶点
        float[][] points = {
                {current_x, current_y},
                {current_x+length, current_y},
                {current_x+length, current_y+width},
                {current_x, current_y+width}
        };

        if (!clockwise) {
            // 如果逆时针飞行，反转顶点顺序
            points = new float[][]{
                    {current_x, current_y},
                    {current_x, current_y+width},
                    {current_x+length, current_y+width},
                    {current_x+length, current_y}
            };
        }

        for (int i = 0; i < 4; i++) {
            int nextIndex = (i + 1) % 4;

            System.out.printf("from %f,%f to %f,%f \n", points[i][0], points[i][1], points[nextIndex][0], points[nextIndex][1]);
            flyPointToPoint(connection, systemId, componentId, points[i][0], points[i][1], points[nextIndex][0], points[nextIndex][1]);

            Thread.sleep(1000);
            // 创建目标位置消息，只在原地旋转90度
            SetPositionTargetLocalNed targetLocal = SetPositionTargetLocalNed.builder()
                    .timeBootMs((int) (timeNowMs))
                    .targetSystem(systemId)
                    .targetComponent(componentId)
                    .coordinateFrame(EnumValue.of(MavFrame.MAV_FRAME_LOCAL_NED))
                    .typeMask(
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_X_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_Y_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_Z_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_VZ_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AX_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AY_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_AZ_IGNORE,
                            PositionTargetTypemask.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE
                    )
                    .x(points[nextIndex][0])
                    .y(points[nextIndex][1])
                    .z(flight_altitude)
                    .vx(0)
                    .vy(0)
                    .afx(0)
                    .afy(0)
                    .yaw(yaw)  // 旋转90度
                    .build();
            connection.send2(systemId, componentId, targetLocal);
            Thread.sleep(1000);
        }
        sendLandCommand(connection, systemId, componentId, 10f);
    }

    public static void yaw(MavlinkConnection connection, int systemId, int componentId,float yaw) throws IOException {

        // Create the SET_ATTITUDE_TARGET message
        SetAttitudeTarget setAttitudeTarget = SetAttitudeTarget.builder()
                .timeBootMs(0)
                .typeMask(AttitudeTargetTypemask.ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE,
                        AttitudeTargetTypemask.ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE,
                        AttitudeTargetTypemask.ATTITUDE_TARGET_TYPEMASK_THRUST_BODY_SET) // Ignore roll, pitch rates and thrust
                .q(eulerToQuaternion(0, 0, yaw))
                .thrust(0)
                .targetSystem(systemId)
                .targetComponent(componentId)
                .build();

        // Send the message
        connection.send2(systemId, componentId, setAttitudeTarget);
    }

    public static void rc(MavlinkConnection connection, int systemId, int componentId,int x,int y,int z,int r) throws IOException {
        ManualControl manualControl = ManualControl.builder()
                .x(x)    // x轴摇杆位置，范围-1000到1000
                .y(y)    // y轴摇杆位置，范围-1000到1000
                .z(z)     // z轴摇杆位置，范围0到1000
                .r(r)     // r轴（偏航）摇杆位置，范围-1000到1000
                .buttons(65535) // 按钮状态
                .buttons2(65535) // 按钮状态
                .target(1)  // 目标系统ID
                .build();
        // Send the message
        connection.send2(systemId, componentId, manualControl);
        System.out.println("ok");
    }
    private static ArrayList<Float> eulerToQuaternion(float roll, float pitch, float yaw) {
        float cy = (float) Math.cos(yaw * 0.5);
        float sy = (float) Math.sin(yaw * 0.5);
        float cp = (float) Math.cos(pitch * 0.5);
        float sp = (float) Math.sin(pitch * 0.5);
        float cr = (float) Math.cos(roll * 0.5);
        float sr = (float) Math.sin(roll * 0.5);

        float w = cr * cp * cy + sr * sp * sy;
        float x = sr * cp * cy - cr * sp * sy;
        float y = cr * sp * cy + sr * cp * sy;
        float z = cr * cp * sy - sr * sp * cy;

        ArrayList<Float> floatList = new ArrayList();
        floatList.add(w);
        floatList.add(x);
        floatList.add(y);
        floatList.add(z);
        return floatList;
    }
}
